#pragma once

#include <chrono>
#include <robot_chassis/CTurnOnRobot.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

/**
 * @brief 里程计节点，负责发布里程计数据和TF变换
 * 
 * 订阅来自STM32的速度数据，通过积分计算位姿，
 * 发布到 /chassis/odom 话题，并广播 odom -> base_link 的TF变换
 */
class CRobotOdometry : public rclcpp::Node
{
  public:
    explicit CRobotOdometry(const std::string& nodeName);
    ~CRobotOdometry();

  private:
    void computeOdometry();
    void publishOdometry();
    void publishTF();
    void callback();

    // ROS2 发布者和定时器
    rclcpp::TimerBase::SharedPtr                          timer_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
    std::unique_ptr<tf2_ros::TransformBroadcaster>        tfBroadcaster_;

    // 数据
    CTurnOnRobot&           turnonRobot_;
    nav_msgs::msg::Odometry odomMsg_;
    SOdometryData           velocityData_;

    // 位姿状态
    double posX_{0.0};      // X位置 (m)
    double posY_{0.0};      // Y位置 (m)
    double theta_{0.0};     // 航向角 (rad)
    rclcpp::Time lastTime_;

    // 参数
    std::string odomFrameId_{"odom"};
    std::string baseFrameId_{"base_link"};
    bool        publishTf_{true};
};
